#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import time
import hmac
import hashlib
import signal
import sys
import re
from datetime import datetime
from threading import Lock

import paho.mqtt.client as mqtt

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data

# ⚠️ 消息包按你的要求改为 my_custom_msgs.msg
from my_custom_msgs.msg import TrajectoryInfo, TimeInfo


def parse_current_time(cur_time: str) -> datetime:
    """
    兼容:
      - '20250824223754'
      - '2025年08月24日22:37:54'
    若仍不匹配，使用正则提取数字并组装。
    """
    if not isinstance(cur_time, str) or not cur_time:
        raise ValueError("empty current_time")
    try:
        return datetime.strptime(cur_time, "%Y%m%d%H%M%S")
    except ValueError:
        pass
    try:
        return datetime.strptime(cur_time, "%Y年%m月%d日%H:%M:%S")
    except ValueError:
        pass
    nums = re.findall(r"\d+", cur_time)
    if len(nums) >= 6:
        y, M, d, H, m, s = map(int, nums[:6])
        return datetime(y, M, d, H, m, s)
    raise ValueError(f"Unsupported current_time format: {cur_time!r}")


def parse_elapsed_time(elap: str) -> datetime:
    """
    解析经过时间为“时间量”的容器（用 datetime 只为复用 .hour/.minute/.second 字段）:
      - 'HH:MM:SS'
      - 'HH时MM分SS秒'
      - 或混合字符串中提取 3 个连续数字
    """
    if not isinstance(elap, str) or not elap:
        raise ValueError("empty elapsed_time")
    for fmt in ("%H:%M:%S",):
        try:
            # 使用 1900-01-01 的哑日期承载时分秒
            return datetime.strptime(elap, fmt)
        except ValueError:
            pass
    # 中文“时分秒”
    m = re.match(r"^\s*(\d+)\s*时\s*(\d+)\s*分\s*(\d+)\s*秒\s*$", elap)
    if m:
        H, M, S = map(int, m.groups())
        return datetime(1900, 1, 1, H, M, S)
    # 回退：提取 3 组数字
    nums = re.findall(r"\d+", elap)
    if len(nums) >= 3:
        H, M, S = map(int, nums[:3])
        return datetime(1900, 1, 1, H, M, S)
    raise ValueError(f"Unsupported elapsed_time format: {elap!r}")


class MqttPublisherNode(Node):
    def __init__(self):
        super().__init__('mqtt_publisher')

        # ====== 状态缓存 ======
        self.current_trajectory = 0
        self.total_executions = 0
        self.current_time_str = ""
        self.elapsed_time_str = ""
        self._lock = Lock()

        # ====== ROS2 订阅 ======
        self.create_subscription(
            TrajectoryInfo, 'trajectory_info', self._trajectory_info_cb,
            qos_profile_sensor_data
        )
        self.create_subscription(
            TimeInfo, 'time_info', self._time_info_cb,
            qos_profile_sensor_data
        )

        # ====== 阿里云 MQTT 配置 ======
        self.broker = "k1dzhervpNg.iot-as-mqtt.cn-shanghai.aliyuncs.com"
        self.port = 1883
        self.topic = "/sys/k1dzhervpNg/Device1/thing/event/property/post"
        self.product_key = "k1dzhervpNg"
        self.device_name = "Device1"
        self.device_secret = "8ae2b4fe8c19a22f933c5900b2701a29"

        # 生成鉴权字段
        self.ts_ms = int(time.time() * 1000)
        self.client_id_str = f"{self.device_name}|securemode=3,signmethod=hmacsha1,timestamp={self.ts_ms}|"
        self.username = f"{self.device_name}&{self.product_key}"
        pwd_plain = (
            f"clientId{self.device_name}"
            f"deviceName{self.device_name}"
            f"productKey{self.product_key}"
            f"timestamp{self.ts_ms}"
        )
        self.password = hmac.new(
            self.device_secret.encode(), pwd_plain.encode(), hashlib.sha1
        ).hexdigest()

        # ====== 创建 MQTT 客户端 ======
        self.client = mqtt.Client(self.client_id_str)
        self.client.username_pw_set(self.username, self.password)
        self.client.on_connect = self._on_connect
        # self.client.on_message = self._on_message
        self.client.on_log = self._on_log
        self._mqtt_connected = False

        try:
            self.client.connect(self.broker, self.port, keepalive=60)
            self.client.loop_start()
        except Exception as e:
            self.get_logger().error(f"MQTT 连接异常: {e}")

        # ====== 定时器：每 3s 发布一次 ======
        self.create_timer(3.0, self._on_timer)
        self.get_logger().info("ROS2 mqtt_publisher 节点已启动。")

    # ---------------- MQTT 回调 ----------------
    def _on_connect(self, client, userdata, flags, rc):
        ok = (rc == 0)
        self._mqtt_connected = ok
        msg = f"MQTT 连接{'成功' if ok else '失败'}"
        print(msg)
        (self.get_logger().info if ok else self.get_logger().error)(msg)
        if ok:
            client.subscribe(self.topic)

    def _on_message(self, client, userdata, msg):
        print(f"收到下行: topic={msg.topic} payload={msg.payload}")

    def _on_log(self, client, userdata, level, buf):
        if level == mqtt.MQTT_LOG_ERR:
            print(f"[MQTT 错误] {buf}")
        elif level == mqtt.MQTT_LOG_WARNING:
            print(f"[MQTT 警告] {buf}")
        # elif level == mqtt.MQTT_LOG_INFO:
        #     print(f"[MQTT 信息] {buf}")

    # ---------------- ROS2 订阅回调 ----------------
    def _trajectory_info_cb(self, msg: TrajectoryInfo):
        with self._lock:
            self.current_trajectory = int(msg.current_trajectory)
            self.total_executions = int(msg.total_executions)

    def _time_info_cb(self, msg: TimeInfo):
        with self._lock:
            self.current_time_str = str(msg.current_time)
            self.elapsed_time_str = str(msg.elapsed_time)

    # ---------------- 定时发布 ----------------
    def _on_timer(self):
        # 取快照
        with self._lock:
            cur_traj = self.current_trajectory
            tot_exec = self.total_executions
            cur_time = self.current_time_str
            elap_time = self.elapsed_time_str
            # 模拟你原来“每圈清空一次”的逻辑
            self.current_time_str = ""
            self.elapsed_time_str = ""

        # 打包 payload
        try:
            # 有效时间则按两种格式解析
            if cur_time and elap_time:
                current_time_dt = parse_current_time(cur_time)
                elapsed_time_dt = parse_elapsed_time(elap_time)

                year = current_time_dt.year % 100
                month = current_time_dt.month
                day = current_time_dt.day
                hour = current_time_dt.hour
                minute = current_time_dt.minute

                hours = elapsed_time_dt.hour
                minutes = elapsed_time_dt.minute
                seconds = elapsed_time_dt.second

                data = bytearray([
                    cur_traj & 0xFF, tot_exec & 0xFF,
                    year, month, day, hour, minute,
                    hours & 0xFF, minutes & 0xFF, seconds & 0xFF,
                    0xcd, 0xdc
                ])
                header_hex = "0g02"   # 按你原协议保留这个“头”（注意它并非标准十六进制）
                data_hex = data.hex()
                full_message = header_hex + data_hex
            else:
                # 设备离线/数据缺失：发送默认数据
                print("设备离线……发送默认数据")
                data = bytearray([
                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x00, 0x00, 0xcd, 0xdc
                ])
                header_hex = "0g02"
                data_hex = data.hex()
                full_message = header_hex + data_hex

        except Exception as e:
            self.get_logger().error(f"打包数据异常: {e}")
            return

        # 发布并打印连接状态+数据
        try:
            if not self._mqtt_connected:
                print("MQTT 未连接（仍尝试 publish）")
            self.client.publish(self.topic, payload=full_message, qos=0, retain=False)
            self.get_logger().info(f"已发送数据: {full_message}")
        except Exception as e:
            self.get_logger().error(f"MQTT 发布异常: {e}")

    # ---------------- 清理 ----------------
    def close(self):
        try:
            self.client.loop_stop()
        except Exception:
            pass
        try:
            self.client.disconnect()
        except Exception:
            pass


def main():
    rclpy.init()
    node = MqttPublisherNode()

    def _sigint_handler(sig, frame):
        print("正在停止...")
        node.close()
        rclpy.shutdown()
        sys.exit(0)

    signal.signal(signal.SIGINT, _sigint_handler)

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.close()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
